#include "MotorsTask.h"
#include "can_task.h"
#include "can_moto_protocol.h"
#include <stm32f4xx.h>
#include "DatStruct.h"
#include "BspM_Driver.h"
#include "surface.h"


static void can_protocol_recv(uint16_t ID,uint8_t *Dat,uint8_t Len);
/*********************************************************************************************
  * @brief  MotorsTask  电机任务
  * @param  void *pvParameters 任务参数指针
  * @retval None
	* @IO None
  ********************************************************************************************/
void MotorsTask(void *pvParameters)
{
	
	surface_bsp_init();
	M1_Init();
	M2_Init();
	CanProtoReg(can_protocol_recv);
	Can_init();
	while(1)
	{
		motor_poll(&TMC4361_M1RegStruct);
		vTaskDelay(1);
		motor_poll(&TMC4361_M2RegStruct);
		vTaskDelay(1);
		
		Can_poll();
	}
}


static void can_protocol_recv(uint16_t ID,uint8_t *Dat,uint8_t Len)
{
	/*Can ID 不符合*/
	if((can_handle.NodeID | FN_SDO_T) != ID)
		return;
	TypeDefTMC4361_RegStruct* handle = NULL;
	can_msg_t ack_msg;
	int reg_data;
	moto_data_t* p_data = (moto_data_t*)Dat;
	
	if(p_data->sdo_addr - MOTOR_ADDR_BASE == 1)
	{
		handle = &TMC4361_M1RegStruct;
	}
	else if(p_data->sdo_addr - MOTOR_ADDR_BASE == 2)
	{
		handle = &TMC4361_M2RegStruct;
	}
	else
	{
		/*电机索引错误 回复错误*/
		can_err_ack(can_handle.NodeID,0xFF,&ack_msg);
		Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
		return;
	}
	if(p_data->sdo_cs == SDO_CS_WRITE)
	{
		/*寄存器写入*/
		motor_write_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),p_data->regaddr,p_data->data,&ack_msg);
		Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
		switch(p_data->regaddr)
		{
			case M_REG_TYPE:break;
			case M_REG_CURREMT_MAX:break;
			case M_REG_INIT_STATUS:reg_data = 1;break;
			case M_REG_HOMING:reg_data = handle->HOMING_FLAG;break;
			case M_REG_CURRENT:motor_current(handle,p_data->data);break;
			case M_REG_MODE:/*只支持位置模式*/break;
			case M_REG_HOME_ACC:break;
			case M_REG_HOME_SPEED:handle->speed_home = p_data->data;break;
			case M_REG_HOME_MODE:
				if(handle->HOMEING_CMD == 0)
				{
					handle->HOMEING_CMD = p_data->data;
					handle->HOMING_FLAG = 0;
//					handle->PROTO_STATUS=handle->PROTO_STATUS&(!M_STATUS_HOME_FINISH);
					motor_home(handle);
					handle->PROTO_STATUS&=(~M_STATUS_HOME_FINISH);
				}
				break;
			case M_REG_START_SPEED:break;
			case M_REG_ACC_A1:break;
			case M_REG_ACC_V1:break;
			case M_REG_ACC_MAX:break;
			case M_REG_SPEED:
				handle->PROTO_V = p_data->data;
				if(handle->HOMEING_CMD == 0)
				{
					handle->bsp_write_reg(0X24,PROTO_V_TO_4361_REG(handle->PROTO_V));
				}
				break;
			case M_REG_MOTION_CURVE:
//			if(p_data->data==0x01){my_curve(handle,1);}
//			if(p_data->data==0x02){my_curve(handle,2);}
//			if(p_data->data==0x03){my_curve(handle,3);}
			my_curve(handle,p_data->data);
					break;
			case M_REG_DEC_D1:break;
			case M_REG_STOP_SPEED:break;
			case M_REG_POS:/*当前位置不支持写*/break;
			case M_REG_TAR_POS:
				motor_tarpos(handle,p_data->data);
				break;
			case M_REG_GLOBAL:break; 
			case M_REG_ERR:handle->bsp_write_reg(0X01,0x00000000);break;
			case M_REG_Limit:handle->LIMIT_SWITCH_CMD = p_data->data;break;
//			case M_REG_SURFACE:break;
			case M_REG_STATUS:/*电机状态寄存器*/break;
			case M_REG_SURFACE_INIT:
				if(p_data->data)
				{
					surface_init();
					handle->SURFACE_STATUS = P_INITING;
				}
			break;
			case M_REG_SURFACE_STATUS:break;
			case M_REG_SURFACE_ENABLE:handle->SURFACE_ENABLE = p_data->data; if(p_data->data!=0){surface_com();} break;
			case M_REG_HALF_CIRCLE:
				handle->X_RANGE = p_data->data;
//				handle->bsp_write_reg(0X36,handle->X_RANGE);
//				handle->bsp_write_reg(0X01,(0x80400000));
				break;
			case M_REG_DISABLE:
				motor_current(handle,0);
				handle->bsp_write_reg(0X6D,0xEC);      // set CHOPCONF reg to initial value,release motor
				handle->bsp_write_reg(0X6C,0x10410150); 
				handle->bsp_nfreeze(1);
				break;
			case M_REG_TCM4361_WRITE:
			{
				debug_write_t* p_debug = (debug_write_t*)Dat;
				handle->bsp_write_reg(p_debug->tmc_addr,p_debug->reg_data); 
			}
			default:break;
		}
	}
	else if(p_data->sdo_cs == SDO_CS_READ)
	{
		/*寄存器读取*/
		reg_data = 0;
		switch(p_data->regaddr)
		{
			case M_REG_TYPE:break;
			case M_REG_CURREMT_MAX:break;
			case M_REG_INIT_STATUS:reg_data = 1;break;
			case M_REG_HOMING:reg_data = handle->HOMING_FLAG;break;
			case M_REG_CURRENT:break;
			case M_REG_MODE:break;
			case M_REG_HOME_ACC:break;
			case M_REG_HOME_SPEED:break;
			case M_REG_HOME_MODE:reg_data = handle->HOMEING_CMD;break;
			case M_REG_START_SPEED:break;
			case M_REG_ACC_A1:break;
			case M_REG_ACC_V1:break;
			case M_REG_ACC_MAX:break;
			case M_REG_SPEED:reg_data = handle->PROTO_V;break;
			case M_REG_DEC_MAX:break;
			case M_REG_DEC_D1:break;
			case M_REG_STOP_SPEED:break;
			case M_REG_POS:reg_data = handle->PROTO_XACTURAL;break;
			case M_REG_TAR_POS:reg_data = handle->PROTO_XTATGET;break;
			case M_REG_GLOBAL:break; 
			case M_REG_ERR:break;
			case M_REG_Limit:;break;
			case M_REG_ENC_POS:
				reg_data = handle->ENC_POS - handle->ENC_HOME_POS;
				break;
			case M_REG_STATUS:/*电机状态寄存器*/
				if(handle->HOMING_FLAG==1)
				{handle->PROTO_STATUS=(handle->PROTO_STATUS|M_STATUS_HOME_FINISH);}	
				else
				{
					handle->PROTO_STATUS&=(~M_STATUS_HOME_FINISH);
				}	
				if((handle->PROTO_STATUS|M_STATUS_IN_POSITION)&&(handle->rewriting==1))
			{
				motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),0x37,handle->XTARGET,&ack_msg);
				Can1SendData(((ack_msg.can_id& ~0xF00) | 0x300),ack_msg.can_data,ack_msg.can_dlc);
				handle->rewriting=0;
			}
			reg_data = handle->PROTO_STATUS;break;
			case M_REG_SURFACE_INIT:break;
			case M_REG_SURFACE_STATUS:reg_data = handle->SURFACE_STATUS;break;
			case M_REG_SURFACE_ENABLE:reg_data = handle->SURFACE_ENABLE;break;
			case M_REG_TCM4361_READ:handle->bsp_read_reg(p_data->data,&reg_data);break;
			default:break;
		}
		motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),p_data->regaddr,reg_data,&ack_msg);
		Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
		if (p_data->regaddr==0xFF)
		{
			motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),0x50,handle->V_ENC,&ack_msg);
			Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
			
			motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),0x21,handle->XACTUAL,&ack_msg);
			Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
			
			motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),0x37,handle->XTARGET,&ack_msg);
			Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
			
			motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),0x36,handle->X_LATCH,&ack_msg);
			Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
			
			motor_read_ack(can_handle.NodeID,SDO_ADDR_TO_MOTOR_INDEX(p_data->sdo_addr),0x0F,handle->STATUS,&ack_msg);
			Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
			
		}
	}
	else
	{
		/*电机命令错误 回复错误*/
		can_err_ack(can_handle.NodeID,0xFF,&ack_msg);
		Can1SendData(ack_msg.can_id,ack_msg.can_data,ack_msg.can_dlc);
		return;
	}
	return;
}
